
/*
    记录下运行过程中的所有变量 放在 
    /home/ubuntu/dev_ws/record 文件夹下
    记录下所有 数据发生的时间

    时间 + 数据
    
*/


#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "modbus/modbus.h"

#include "rclcpp/rclcpp.hpp"
#include "basic_sprayer_interfaces/msg/four_drivers.hpp" // 四个驱动
#include "basic_sprayer_interfaces/msg/four_wheel.hpp"   // 四轮速度
#include "basic_sprayer_interfaces/msg/adc.hpp"          // adc的Message
#include "basic_sprayer_interfaces/msg/human_cmd.hpp"    // 控制信号
#include "basic_sprayer_interfaces/msg/pid.hpp"          // 四轮速度
#include "sensor_msgs/msg/imu.hpp"    

#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <stdio.h>
#include <errno.h> 



/*
    以后可以标准化命名
    cmd/x             // 输入指令
    sensors/x          // 传感器
    drivers/x          // 驱动器
*/

/* 记录数据 */
class recordNode : public rclcpp::Node
{
public:
    recordNode() : Node("recordNode")
    {
        /* 订阅adc */
        subscription_adc = this->create_subscription<basic_sprayer_interfaces::msg::Adc>(
            "adc", 10, std::bind(&recordNode::callback_adc, this, std::placeholders::_1));
        /* 订阅车身状态 */
        subscription_driver = this->create_subscription<basic_sprayer_interfaces::msg::FourDrivers>(
            "driver/status", 10, std::bind(&recordNode::callback_status, this, std::placeholders::_1));
        /*订阅发布的命令*/
        subscription_cmd = this->create_subscription<basic_sprayer_interfaces::msg::HumanCmd>(
            "humancmd", 10, std::bind(&recordNode::callback_cmd, this, std::placeholders::_1));
        /* fourwheel */
        subscription_fourwheel = this->create_subscription<basic_sprayer_interfaces::msg::FourWheel>(
            "driver/fourwheel", 10, std::bind(&recordNode::callback_fourwheel, this, std::placeholders::_1));
        subscription_imu = this->create_subscription<sensor_msgs::msg::Imu>(
            "mpu6050", 10, std::bind(&recordNode::callback_imu, this, std::placeholders::_1));
        
        file_create();
    }
    /* 创建对应的文件 */
    void file_create(void){
        /* 新建文件夹 */
        timespec time;
        clock_gettime(CLOCK_REALTIME, &time); //获取相对于1970到现在的秒数
        tm nowTime;
        localtime_r(&time.tv_sec, &nowTime);
        /* 生成路径 */
        sprintf(file_path, "/home/ubuntu/dev_ws/record/%04d%02d%02d%02d%02d%02d", nowTime.tm_year + 1900, nowTime.tm_mon, nowTime.tm_mday, nowTime.tm_hour, nowTime.tm_min, nowTime.tm_sec);
        RCLCPP_INFO(this->get_logger(), "path:%s",file_path);   //打印路径
        if(access(file_path,0)==-1)         // access函数是查看文件夹是不是存在
        {
            RCLCPP_INFO(this->get_logger(), "path do not exist!");   //打印路径
            if (mkdir(file_path,0777))      // 如果不存在就用mkdir函数来创建
            {
                RCLCPP_INFO(this->get_logger(), "dir create failed! errno = %s",strerror(errno));   //打印路径
            }
        }
        /* 创建5个文件  然后在对应的地方写入 */
        sprintf(file_adc,"%s/adc.csv",file_path);
        fd_adc = fopen(file_adc,"w+");
        // fprintf(fd_adc,"%lf\n",this->now().seconds());
        fprintf(fd_adc,"seconds,front_raw,rear_raw,front_voltage,rear_voltage,front_angle,rear_angle\n");

        sprintf(file_imu,"%s/imu.csv",file_path);
        fd_imu = fopen(file_imu,"w+");
        // fprintf(fd_adc,"%lf\n",this->now().seconds());
        fprintf(fd_imu,"seconds,angular_velocity_x,angular_velocity_y,angular_velocity_z,linear_acceleration_x,linear_acceleration_y,linear_acceleration_z,orientation_w,orientation_x,orientation_y,orientation_z\n");
        /* 4个电机状态 */
        sprintf(file_status,"%s/drivers_status.csv",file_path);
        fd_status = fopen(file_status,"w+");
        fprintf(fd_status,"seconds,id,bus_voltage_v,current_a,speed_rpm,temperature_c,power_w(error don't use),position_rad(useless),error_code\n");

        /* cmd */
        sprintf(file_cmd,"%s/cmd.csv",file_path);
        fd_cmd = fopen(file_cmd,"w+");
        fprintf(fd_cmd,"seconds,cmd_switch(1 denote start),velocity(m/s),angle(deg)\n");
        /* 四个车轮的给定速度 */
        sprintf(file_four_wheel,"%s/four_wheel.csv",file_path);
        fd_four_wheel = fopen(file_four_wheel,"w+");
        fprintf(fd_four_wheel,"seconds,start_flag(1 denote start),output_left_front(rpm),output_right_front(rpm),output_left_rear(rpm),output_right_rear(rpm)\n");

    }
    /* 关闭对应的文件 */
    void file_close(void){
        if(fd_adc != NULL){
            fclose(fd_adc);
        }
        if(fd_imu != NULL){
            fclose(fd_imu);
        }
        if(fd_status != NULL){
            fclose(fd_status);
        }
        if(fd_cmd != NULL){
            fclose(fd_cmd);
        }
        if(fd_four_wheel != NULL){
            fclose(fd_four_wheel);
        }
    }

    ~recordNode(){

    }
private:
    rclcpp::Subscription<basic_sprayer_interfaces::msg::FourWheel>::SharedPtr subscription_fourwheel;   // 四轮给定目标
    rclcpp::Subscription<basic_sprayer_interfaces::msg::FourDrivers>::SharedPtr subscription_driver;    // 驱动器返回数据
    rclcpp::Subscription<basic_sprayer_interfaces::msg::Adc>::SharedPtr subscription_adc;               // adc 数据
    rclcpp::Subscription<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr subscription_cmd;          // 命令数据
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_imu;          // mpu6050数据

    char file_path[512];

    char file_adc[512];
    char file_imu[512];
    char file_status[512];
    char file_four_wheel[512];
    char file_cmd[512];

    FILE *fd_adc;
    FILE *fd_imu;
    FILE *fd_status;
    FILE *fd_cmd;
    FILE *fd_four_wheel;

    /*订阅adc*/
    void callback_adc(const basic_sprayer_interfaces::msg::Adc::SharedPtr msg)
    {
        rclcpp::Time now(msg->header.stamp.sec,msg->header.stamp.nanosec);
        fprintf(fd_adc,"%lf,%d,%d,%f,%f,%f,%f\n",
        now.seconds(),
        msg->channle_raw[0],msg->channle_raw[1],
        msg->voltage_filter[0],msg->voltage_filter[1],
        msg->angle_deg[0],msg->angle_deg[1]
        );
    }
    /* 订阅 四个车轮的状态 */
    void callback_status(const basic_sprayer_interfaces::msg::FourDrivers::SharedPtr msg)
    {
        rclcpp::Time now(msg->header.stamp.sec,msg->header.stamp.nanosec);
        for(uint8_t i = 0; i < 4;i ++){
            fprintf(fd_status,"%lf,%d,%f,%f,%f,%f,%f,%f,%d\n",
            now.seconds(),
            i+1,
            msg->driver[i].bus_voltage_v,
            msg->driver[i].current_a,
            msg->driver[i].speed_rpm,
            msg->driver[i].temperature_c,
            msg->driver[i].power_w,
            msg->driver[i].position_rad,
            msg->driver[i].error_code
            );            
        }
    }
    /* 订阅 输入状态 */
    void callback_cmd(const basic_sprayer_interfaces::msg::HumanCmd::SharedPtr msg)
    {
        rclcpp::Time now(msg->header.stamp.sec,msg->header.stamp.nanosec);
        fprintf(fd_cmd,"%lf,%d,%f,%f\n",
        now.seconds(),
        msg->cmd_switch,
        msg->velocity,
        msg->angle
        );
    }
    /* 订阅 四轮命令数据 */
    void callback_fourwheel(const basic_sprayer_interfaces::msg::FourWheel::SharedPtr msg)
    {
        rclcpp::Time now(msg->header.stamp.sec,msg->header.stamp.nanosec);
        fprintf(fd_four_wheel,"%lf,%d,%f,%f,%f,%f\n",
        now.seconds(),
        msg->start_flag,
        msg->output_left_front,
        msg->output_right_front,
        msg->output_left_rear,
        msg->output_right_rear
        );
    }
    /* 订阅 imu数据 */
    void callback_imu(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        rclcpp::Time now(msg->header.stamp.sec,msg->header.stamp.nanosec);
        fprintf(fd_imu,"%lf,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
        now.seconds(),
        msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z,
        msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z,
        msg->orientation.w,msg->orientation.x,msg->orientation.y,msg->orientation.z
        );
    }
};


int main(int argc, char *argv[])
{
    /*创建 ros */
    rclcpp::init(argc, argv);
    auto pub_node = std::make_shared<recordNode>();
    rclcpp::spin(pub_node);
    pub_node->file_close();
    /*创建线程*/
    rclcpp::shutdown();
}

